/*
 * (C) Copyright 2023-2027 QM Electronics Co., Ltd
 *
 * SPDX-License-Identifier:	GPL-2.0+
 */

#include <common.h>
#include <malloc.h>
#include <asm/arch/nvm_vendor.h>
//#include <boot_rkimg.h>
#include <linux/mtd/mtd.h>

#include <nand.h>
#include <part.h>


/* tag for vendor check */
#define NVM_VENDOR_TAG		0x4e565644       //  NVVD ASCII code:'N':0x4e, 'V':0x56, 'V':0x56, 'D':0x44
/* The Vendor partition contains the number of Vendor blocks */
#define MTD_VENDOR_PART_NUM	1
#define NAND_VENDOR_PART_NUM	4
#define VENDOR_PART_NUM		4
/* align to 64 bytes */
#define VENDOR_BTYE_ALIGN	0x3F
#define VENDOR_BLOCK_SIZE	512



/* --- Spi Nand/SLC/MLC large capacity case define --- */

/* The Vendor partition contains the number of Vendor blocks */
#define NAND_NVVM_PART_ADDR        0x00200000
#define NAND_NVVM_PART_SIZE        0x00100000
#define NAND_NVVM_PART_BLKS        ((NAND_NVVM_PART_SIZE) / (VENDOR_BLOCK_SIZE))

#define NAND_NVVM_PART_OFFSET		((NAND_NVVM_PART_ADDR) / (VENDOR_BLOCK_SIZE))
/*
 * The number of memory blocks used by each
 * Vendor structure(256 * 512B = 128KB)
 */
#define NAND_VENDOR_PART_BLKS		128
/* The maximum number of items in each Vendor block */
#define NAND_VENDOR_ITEM_NUM		126



/* Vendor uinit test define */
int vendor_storage_test(void);

struct vendor_hdr {
	u32	tag;
	u32	version;
	u16	next_index;
	u16	item_num;
	u16	free_offset; /* Free space offset */
	u16	free_size; /* Free space size */
};

/*
 * Different types of Flash vendor info are different.
 * EMMC:EMMC_VENDOR_PART_BLKS * BLOCK_SIZE(512) = 64KB;
 * Spi Nor/Spi Nand/SLC/MLC: FLASH_VENDOR_PART_BLKS *
 * BLOCK_SIZE(512) = 4KB.
 * hash: For future expansion.
 * version2: Together with hdr->version, it is used to
 * ensure the current Vendor block content integrity.
 *   (version2 == hdr->version):Data valid;
 *   (version2 != hdr->version):Data invalid.
 */
struct vendor_info {
	struct vendor_hdr *hdr;
	struct vendor_item *item;
	u8 *data;
	u32 *hash;
	u32 *version2;
};

struct mtd_flash_info {
	u32 part_offset;
	u32 part_size;
	u32 blk_offset;
	u32 page_offset;
	u32 version;
	u32 ops_size;
	u32 blk_size;
};


struct mtd_nand_info {
	u32 blk_offset;
	u32 page_offset;
	u32 version;
	u32 ops_size;
	u32 partsize;
	u32 partoffset;
};


static const char *vendor_mtd_name = "padding";
static struct mtd_nand_info mtdnand_info;



/*
 * Calculate the offset of each field for spi nand/slc/mlc large capacity case.
 * Flash vendor info size: 128KB
 */
#define NAND_VENDOR_INFO_SIZE	(NAND_VENDOR_PART_BLKS * VENDOR_BLOCK_SIZE)
#define NAND_VENDOR_DATA_OFFSET	(sizeof(struct vendor_hdr) + NAND_VENDOR_ITEM_NUM * sizeof(struct vendor_item))
#define NAND_VENDOR_HASH_OFFSET (NAND_VENDOR_INFO_SIZE - 8)
#define NAND_VENDOR_VERSION2_OFFSET (NAND_VENDOR_INFO_SIZE - 4)



/* vendor info */
static struct vendor_info vendor_info;
/* The storage type of the device */
static int bootdev_type;

static struct mtd_flash_info s_flash_info;


struct mtd_info *mtd;




#ifdef CONFIG_MTD_BLK

static const char *vendor_mtd_name = "vnvm";
#endif

/* vendor private read write ops*/
static	int (*_flash_read)(struct blk_desc *dev_desc,
			   u32 sec,
			   u32 n_sec,
			   void *buffer);
static	int (*_flash_write)(struct blk_desc *dev_desc,
			    u32 sec,
			    u32 n_sec,
			    void *buffer);

int flash_vendor_dev_ops_register(int (*read)(struct blk_desc *dev_desc,
					      u32 sec,
					      u32 n_sec,
					      void *p_data),
				  int (*write)(struct blk_desc *dev_desc,
					       u32 sec,
					       u32 n_sec,
					       void *p_data))
{
	if (!_flash_read) {
		_flash_read = read;
		_flash_write = write;
		return 0;
	}

	return -EPERM;
}



static	int spinand_blk_read(struct mtd_info *mtd,
			   u32 sec,
			   u32 n_sec,
			   void *buffer)
{
	ulong off;
	int ret = 0;

	size_t size;
	// calcute read offset and size
	off = sec * VENDOR_BLOCK_SIZE;
	size = n_sec * VENDOR_BLOCK_SIZE;

	ret = nand_read_skip_bad(mtd, off, &size,
							 (u_char *)buffer);

	return ret == 0 ? 0 : 1;


}


// spinand erase and write, skip bad block
static	int spinand_blk_write(struct mtd_info *mtd,
			    u32 sec,
			    u32 n_sec,
			    void *buffer)
{
	nand_erase_options_t opts;
	ulong off;
	int ret, offset;
	int bad_block_size;
	size_t size;


	// check if there are enough space left to write. 
	bad_block_size = 0;
	for (offset = sec; offset < (s_flash_info.part_size + offset); offset += s_flash_info.blk_size) {
		if (mtd_block_isbad(mtd, (s_flash_info.part_offset + offset) << 9))
			bad_block_size += s_flash_info.blk_size;
	}

	if ((sec + bad_block_size + n_sec) > (s_flash_info.part_size + s_flash_info.part_offset)) {
		printf("spinand_blk_write error: no enough space left!\n");
		return 1;
	}

	// calcute write offset and size
	off = sec * VENDOR_BLOCK_SIZE;
	size = n_sec * VENDOR_BLOCK_SIZE;

	//erase block
	memset(&opts, 0, sizeof(opts));
	opts.offset = off;
	opts.length = size;
	opts.quiet = 1;
	//skip bad block
	opts.spread = 1;

	// erase block
	ret = nand_erase_opts(mtd, &opts);
	if(ret) {
		printf("SPINand flash erase error: '0x%llx'.\n", opts.offset);
		return 1;
	}		
	//write block
	ret = nand_write_skip_bad(mtd, off, &size,
							  (u_char *)buffer);

	return ret == 0 ? 0 : 1;

}

static	int spinand_nvmvendor_init(void)
{
	struct mtd_info *mtd;
	int ret, offset;
	int bad_block_size;
	void *buf = vendor_info.hdr;


	mtd = &nand_info[nand_curr_device];

	s_flash_info.part_offset = (u32)NAND_NVVM_PART_OFFSET;
	s_flash_info.part_size = (u32)NAND_NVVM_PART_BLKS;
	s_flash_info.page_offset = 0;
	s_flash_info.blk_offset = 0;
	s_flash_info.version = 0;

	s_flash_info.blk_size = mtd->erasesize >> 9;
	s_flash_info.ops_size = roundup(NAND_VENDOR_INFO_SIZE, mtd->writesize) >> 9;

	/* scan bad block and calculate the real size can be used */
	bad_block_size = 0;
	for (offset = 0; offset < s_flash_info.part_size; offset += s_flash_info.blk_size) {
		if (mtd_block_isbad(mtd, (s_flash_info.part_offset + offset) << 9))
			bad_block_size += s_flash_info.blk_size;
	}
	s_flash_info.part_size -= bad_block_size;

	for (offset = 0; offset < s_flash_info.part_size; offset += s_flash_info.blk_size) {
		ret = spinand_blk_read(mtd, s_flash_info.part_offset + offset,
				NAND_VENDOR_INFO_SIZE >> 9,
				(u8 *)buf);
		debug("%s: read %x version = %x\n", __func__,
		      s_flash_info.part_offset + offset,
		      vendor_info.hdr->version);
		if (ret == (NAND_VENDOR_INFO_SIZE >> 9) && vendor_info.hdr->tag == NVM_VENDOR_TAG &&
		    vendor_info.hdr->version == *vendor_info.version2) {
			if (vendor_info.hdr->version > s_flash_info.version) {
				s_flash_info.version = vendor_info.hdr->version;
				s_flash_info.blk_offset = offset;
			}
		}
	}

	debug("%s: s_flash_info.version = %x %x\n", __func__, s_flash_info.version, s_flash_info.blk_offset);
	if (s_flash_info.version) {
		for (offset = s_flash_info.blk_size  - s_flash_info.ops_size;
		     offset >= 0;
		     offset -= s_flash_info.ops_size) {
			ret = spinand_blk_read(mtd, s_flash_info.part_offset +
					s_flash_info.blk_offset + offset,
					1,
					(u8 *)buf);

			/* the page is not programmed */
			if (ret == 1 && vendor_info.hdr->tag == 0xFFFFFFFF)
				continue;

			/* point to the next free page */
			if (s_flash_info.page_offset < offset)
				s_flash_info.page_offset = offset + s_flash_info.ops_size;

			if (ret != 1 || vendor_info.hdr->tag != NVM_VENDOR_TAG)
				continue;
			ret = spinand_blk_read(mtd, s_flash_info.part_offset +
					s_flash_info.blk_offset + offset,
					NAND_VENDOR_INFO_SIZE >> 9,
					(u8 *)buf);
			debug("%s: read %x version = %x\n", __func__,
			      s_flash_info.part_offset + s_flash_info.blk_offset  + offset,
			      vendor_info.hdr->version);

			if (ret == (NAND_VENDOR_INFO_SIZE >> 9)  && vendor_info.hdr->tag == NVM_VENDOR_TAG &&
			    vendor_info.hdr->version == *vendor_info.version2) {
				s_flash_info.version = vendor_info.hdr->version;
				break;
			}
		}
	} else {
		memset((u8 *)vendor_info.hdr, 0, NAND_VENDOR_INFO_SIZE);
		vendor_info.hdr->version = 1;
		vendor_info.hdr->tag = NVM_VENDOR_TAG;
		vendor_info.hdr->free_size =
			((u32)(size_t)vendor_info.hash
			- (u32)(size_t)vendor_info.data);
		*vendor_info.version2 = vendor_info.hdr->version;
	}

	return 0;

}

#ifdef CONFIG_MTD_BLK
static int mtd_vendor_storage_init(struct blk_desc *dev_desc)
{
	struct mtd_info *mtd = (struct mtd_info *)dev_desc->bdev->priv;
	disk_partition_t vnvm_part_info;
	void *buf = vendor_info.hdr;
	int ret, offset;
	int part_num, bad_block_size;

	memset(&vnvm_part_info, 0x0, sizeof(vnvm_part_info));
	part_num = part_get_info_by_name(dev_desc, vendor_mtd_name, &vnvm_part_info);
	if (part_num < 0)
		return -EIO;

	s_flash_info.part_offset = (u32)vnvm_part_info.start;
	s_flash_info.part_size = (u32)vnvm_part_info.size;
	s_flash_info.page_offset = 0;
	s_flash_info.blk_offset = 0;
	s_flash_info.version = 0;
	/* SPI Nor unified to Support 64KB erase block */
	if (dev_desc->devnum == BLK_MTD_SPI_NOR)
		s_flash_info.blk_size = 0x80;
	else
		s_flash_info.blk_size = mtd->erasesize >> 9;
	s_flash_info.ops_size = roundup(FLASH_VENDOR_INFO_SIZE, mtd->writesize) >> 9;

	/* scan bad block and calculate the real size can be used */
	bad_block_size = 0;
	for (offset = 0; offset < s_flash_info.part_size; offset += s_flash_info.blk_size) {
		if (mtd_block_isbad(mtd, (s_flash_info.part_offset + offset) << 9))
			bad_block_size += s_flash_info.blk_size;
	}
	s_flash_info.part_size -= bad_block_size;

	for (offset = 0; offset < s_flash_info.part_size; offset += s_flash_info.blk_size) {
		ret = blk_dread(dev_desc, s_flash_info.part_offset + offset,
				FLASH_VENDOR_INFO_SIZE >> 9,
				(u8 *)buf);
		debug("%s: read %x version = %x\n", __func__,
		      s_flash_info.part_offset + offset,
		      vendor_info.hdr->version);
		if (ret == (FLASH_VENDOR_INFO_SIZE >> 9) && vendor_info.hdr->tag == NVM_VENDOR_TAG &&
		    vendor_info.hdr->version == *vendor_info.version2) {
			if (vendor_info.hdr->version > s_flash_info.version) {
				s_flash_info.version = vendor_info.hdr->version;
				s_flash_info.blk_offset = offset;
			}
		}
	}

	debug("%s: s_flash_info.version = %x %x\n", __func__, s_flash_info.version, s_flash_info.blk_offset);
	if (s_flash_info.version) {
		for (offset = s_flash_info.blk_size  - s_flash_info.ops_size;
		     offset >= 0;
		     offset -= s_flash_info.ops_size) {
			ret = blk_dread(dev_desc, s_flash_info.part_offset +
					s_flash_info.blk_offset + offset,
					1,
					(u8 *)buf);

			/* the page is not programmed */
			if (ret == 1 && vendor_info.hdr->tag == 0xFFFFFFFF)
				continue;

			/* point to the next free page */
			if (s_flash_info.page_offset < offset)
				s_flash_info.page_offset = offset + s_flash_info.ops_size;

			if (ret != 1 || vendor_info.hdr->tag != NVM_VENDOR_TAG)
				continue;
			ret = blk_dread(dev_desc, s_flash_info.part_offset +
					s_flash_info.blk_offset + offset,
					FLASH_VENDOR_INFO_SIZE >> 9,
					(u8 *)buf);
			debug("%s: read %x version = %x\n", __func__,
			      s_flash_info.part_offset + s_flash_info.blk_offset  + offset,
			      vendor_info.hdr->version);

			if (ret == (FLASH_VENDOR_INFO_SIZE >> 9)  && vendor_info.hdr->tag == NVM_VENDOR_TAG &&
			    vendor_info.hdr->version == *vendor_info.version2) {
				s_flash_info.version = vendor_info.hdr->version;
				break;
			}
		}
	} else {
		memset((u8 *)vendor_info.hdr, 0, FLASH_VENDOR_INFO_SIZE);
		vendor_info.hdr->version = 1;
		vendor_info.hdr->tag = NVM_VENDOR_TAG;
		vendor_info.hdr->free_size =
			((u32)(size_t)vendor_info.hash
			- (u32)(size_t)vendor_info.data);
		*vendor_info.version2 = vendor_info.hdr->version;
	}

	return 0;
}

static int mtd_vendor_write(struct blk_desc *dev_desc,
			    u32 sec,
			    u32 n_sec,
			    void *buf)
{
	int ret, count = 0, err = 0;

re_write:
	debug("[Vendor INFO]:%s page_offset=0x%x count = %x\n", __func__, s_flash_info.part_offset +
	      s_flash_info.blk_offset + s_flash_info.page_offset, count);
	if (s_flash_info.page_offset >= s_flash_info.blk_size) {
		s_flash_info.blk_offset += s_flash_info.blk_size;
		if (s_flash_info.blk_offset >= s_flash_info.part_size)
			s_flash_info.blk_offset = 0;
		s_flash_info.page_offset = 0;
	}

	dev_desc->op_flag |= BLK_MTD_CONT_WRITE;
	ret = blk_dwrite(dev_desc, s_flash_info.part_offset +
			 s_flash_info.blk_offset + s_flash_info.page_offset,
			 FLASH_VENDOR_INFO_SIZE >> 9,
			 (u8 *)buf);
	dev_desc->op_flag &= ~(BLK_MTD_CONT_WRITE);

	s_flash_info.page_offset += s_flash_info.ops_size;
	if (ret != (FLASH_VENDOR_INFO_SIZE >> 9)) {
		err++;
		if (err > 3)
			return -EIO;
		goto re_write;
	}

	count++;
	/* write 2 copies for reliability */
	if (count < 2)
		goto re_write;

	return ret;
}
#endif

/**********************************************************/
/*              vendor API implementation                 */
/**********************************************************/
static int vendor_ops(u8 *buffer, u32 addr, u32 n_sec, int write)
{
	unsigned int lba = 0;
	int ret = 0;

	struct mtd_info *mtd;
	//int part_num, bad_block_size;

	mtd = &nand_info[nand_curr_device];
	if (!mtd) {
		printf("%s: dev_desc is NULL!\n", __func__);
		return -ENODEV;
	}
	/* Get the offset address according to the device type */

	// IF_TYPE_SPINAND:
		/*
		 * The location of VendorStorage in Flash is shown in the
		 * following figure. The starting address of the VendorStorage
		 * partition offset is 0KB in FTL vendor block,
		 * and the partition size is 128KB.
		 * ----------------------------------------------------
		 * |  2MB        |     VendorStorage                |
		 * ----------------------------------------------------
		 */
		lba = NAND_NVVM_PART_OFFSET;
		debug("[Vendor INFO]:VendorStorage offset address=0x%x\n", lba);


	if (write) {
		ret = spinand_blk_write(mtd, lba + addr, n_sec, buffer);

	} else {

		ret = spinand_blk_read(mtd, lba + addr, n_sec, buffer);

	}

	debug("[Vendor INFO]:op=%s, ret=%d\n", write ? "write" : "read", ret);

	return ret;
}

int nandpart_block_isbad(struct mtd_info *mtd, loff_t ofs)
{
	int ret;
	uint64_t addr;
	//get nand offset
	addr = mtdnand_info.partoffset + ofs;
	//printf("[Vendor INFO]:nandpart_block_isbad, offset=0x%llx\n", addr);
	ret = nand_block_isbad(mtd, addr);
	return ret;

}

int nandpart_write(struct mtd_info *mtd, loff_t to, size_t len, size_t *retlen,
	const u_char *buf)
{
	int ret;
	uint64_t addr;

	//get nand offset
	addr = mtdnand_info.partoffset + to;
	printf("[Vendor INFO]:nandpart_write, offset=0x%llx\n", addr);
	ret =mtd_write(mtd, addr, len,
		retlen, (u8 *)buf);

	return ret;


}

int nandpart_read(struct mtd_info *mtd, loff_t from, size_t len, size_t *retlen,
	u_char *buf)
{
	int ret;
	uint64_t addr;
	//get nand offset
	addr = mtdnand_info.partoffset + from;
	printf("[Vendor INFO]:nandpart_read, addr=0x%llx, len=0x%x\n", addr, len);
	ret =mtd_read(mtd, addr, len,
		retlen, (u8 *)buf);

	return ret;

}

int nandpart_erase(struct mtd_info *mtd, struct erase_info *instr)
{
	int ret;

	instr->addr += mtdnand_info.partoffset;
	printf("[Vendor INFO]:nandpart_erase, addr=0x%llx\n", instr->addr);
	ret = mtd_erase(mtd, instr);

	return ret;
}


static int mtd_vendor_nand_write(struct mtd_info *mtd, u8 *buffer)
{
	size_t bytes_write;
	int err, count = 0;
	struct erase_info ei;

re_write:
	if (mtdnand_info.page_offset >= mtd->erasesize) {
		mtdnand_info.blk_offset += mtd->erasesize;
		if (mtdnand_info.blk_offset >= mtdnand_info.partsize)
			mtdnand_info.blk_offset = 0;
		if (nandpart_block_isbad(mtd, mtdnand_info.blk_offset))
			goto re_write;

		memset(&ei, 0, sizeof(struct erase_info));
		ei.addr = mtdnand_info.blk_offset;
		ei.len	= mtd->erasesize;
		ei.mtd = mtd;
		if (nandpart_erase(mtd, &ei))
			goto re_write;

		mtdnand_info.page_offset = 0;
	}

	err = nandpart_write(mtd, mtdnand_info.blk_offset + mtdnand_info.page_offset,
			mtdnand_info.ops_size, &bytes_write, (u8 *)buffer);
	mtdnand_info.page_offset += mtdnand_info.ops_size;
	if (err)
		goto re_write;

	count++;
	/* write 2 copies for reliability */
	if (count < 2)
		goto re_write;

	return 0;
}
void vendorstorage_dump(void)
{
	u32 i,j; 
	u16 offset, size;
	
	struct vendor_item *item;
	char pbuf[64];
	
	printf("#######vendorstorage_dump start ######\n");
	printf("mtdnand_info.blk_offset: 0x%x\n", mtdnand_info.blk_offset);
	printf("hdr:version:%d, next_index:%d, item_num:%d,free_offset:0x%x, free_size:%d\n",
		vendor_info.hdr->version, vendor_info.hdr->next_index, vendor_info.hdr->item_num, 
		vendor_info.hdr->free_offset, vendor_info.hdr->free_size);
	
	// dump all the item data
	item = vendor_info.item;
	for (i = 0; i < vendor_info.hdr->item_num; i++) {
		offset = (item + i)->offset;
		if ((item + i)->size > 32)
		{
			size = 32;
		}
		else
		{
			size = (item + i)->size;
		}
		memcpy(pbuf, (vendor_info.data + offset), size);
		printf("item %d, id %d\n", i, (item + i)->id);
		for (j = 0; j < size; j++)
		{
			printf(" 0x%x,", pbuf[j]);
		}
		printf("\n");
		
	}

	printf("#######vendorstorage_dump end ######\n");

}

/*
 * The VendorStorage partition is divided into four parts
 * (vendor 0-3) and its structure is shown in the following figure.
 * The init function is used to select the latest and valid vendor.
 *
 * |******************** FLASH ********************|
 * -------------------------------------------------
 * |  vendor0  |  vendor1  |  vendor2  |  vendor3  |
 * -------------------------------------------------
 * Notices:
 *   1. "version" and "version2" are used to verify that the vendor
 *      is valid (equal is valid).
 *   2. the "version" value is larger, indicating that the current
 *      verndor data is new.
 */
int vendor_storage_init(void)
{
	int ret = 0;
	int err, offset;
	u8 *buffer;
	u32 size;
	u16 data_offset, hash_offset, part_num;
	u16 version2_offset, part_size;

	struct erase_info ei;
	size_t bytes_read;


	bootdev_type = IF_TYPE_SPINAND;
	mtd = &nand_info[nand_curr_device];

	printf("vendor_storage_init-001\n");
	if (!mtd) {
		printf("[Vendor ERROR]:Invalid boot device type\n");
		return -ENODEV;
	}

	//IF_TYPE_SPINAND:
		size = NAND_VENDOR_INFO_SIZE;
		part_size = NAND_VENDOR_PART_BLKS;
		data_offset = NAND_VENDOR_DATA_OFFSET;
		hash_offset = NAND_VENDOR_HASH_OFFSET;
		version2_offset = NAND_VENDOR_VERSION2_OFFSET;
		part_num = NAND_VENDOR_PART_NUM;

	/* Always use, no need to release */
	buffer = (u8 *)malloc(size);
	if (!buffer) {
		printf("[Vendor ERROR]:Malloc failed!\n");
		ret = -ENOMEM;
		goto out;
	}
	/* Pointer initialization */
	vendor_info.hdr = (struct vendor_hdr *)buffer;
	vendor_info.item = (struct vendor_item *)(buffer + sizeof(struct vendor_hdr));
	vendor_info.data = buffer + data_offset;
	vendor_info.hash = (u32 *)(buffer + hash_offset);
	vendor_info.version2 = (u32 *)(buffer + version2_offset);

	printf("vendor_storage_init-002-mtdsize = 0x%llx,0x%x, 0x%x\n", mtd->size, mtd->erasesize, mtd->writesize);

	// init erasesize. writesize
	if (mtd->erasesize == 0)
	{
		mtd->erasesize = 0x20000;
	}

	if (mtd->writesize == 0)
	{
		mtd->writesize = 0x800;
	}

	mtdnand_info.page_offset = 0;
	mtdnand_info.blk_offset = 0;
	mtdnand_info.version = 0;
	mtdnand_info.ops_size = (NAND_VENDOR_INFO_SIZE + mtd->writesize - 1) / mtd->writesize;
	mtdnand_info.ops_size *= mtd->writesize;
	mtdnand_info.partsize = NAND_NVVM_PART_SIZE;
	mtdnand_info.partoffset = NAND_NVVM_PART_ADDR;

	printf("vendor_storage_init-02-mtdnand_info.partsize = 0x%x\n", mtdnand_info.partsize);
	/* Find valid and up-to-date one from (vendor0 - vendor3) */
	for (offset = 0; offset < mtdnand_info.partsize; offset += mtd->erasesize) {

		printf("vendor_storage_init-0021 0x%x\n", offset);
		if (!nandpart_block_isbad(mtd, offset)) {

			err = nandpart_read(mtd, offset, NAND_VENDOR_INFO_SIZE,
				       &bytes_read, (u8 *)vendor_info.hdr);
			if (err && err != -EUCLEAN)
				continue;
			if (bytes_read == NAND_VENDOR_INFO_SIZE &&
			    vendor_info.hdr->tag == NVM_VENDOR_TAG &&
				vendor_info.hdr->version == *(vendor_info.version2)) {
				if (vendor_info.hdr->version > mtdnand_info.version) {
					mtdnand_info.version = vendor_info.hdr->version;
					mtdnand_info.blk_offset = offset;
				}
			}
		} else if (mtdnand_info.blk_offset == offset)
			mtdnand_info.blk_offset += mtd->erasesize;
	}
	printf("mtdnand_info.version: %d\n", mtdnand_info.version);
	if (mtdnand_info.version) {
		printf("vendor_storage_init-003 \n");
		for (offset = mtd->erasesize - mtdnand_info.ops_size;
			offset >= 0;
			offset -= mtdnand_info.ops_size) {
		   err = nandpart_read(mtd, mtdnand_info.blk_offset + offset,
					  NAND_VENDOR_INFO_SIZE,
					  &bytes_read,
					  (u8 *)vendor_info.hdr);

		   /* the page is not programmed */
		   if (!err && bytes_read == NAND_VENDOR_INFO_SIZE &&
			   vendor_info.hdr->tag == 0xFFFFFFFF &&
			   vendor_info.hdr->version == 0xFFFFFFFF &&
			   *(vendor_info.version2) == 0xFFFFFFFF)
			   continue;

		   /* point to the next free page */
		   if (mtdnand_info.page_offset < offset)
			   mtdnand_info.page_offset = offset + mtdnand_info.ops_size;

		   /* ecc error or io error */
		   if (err && err != -EUCLEAN)
			   continue;

		   if (bytes_read == NAND_VENDOR_INFO_SIZE &&
			   vendor_info.hdr->tag == NVM_VENDOR_TAG &&
			   vendor_info.hdr->version == *(vendor_info.version2)) {
			   mtdnand_info.version = vendor_info.hdr->version;

			   vendorstorage_dump();
			   break;

		   }
	   }
	} else {
		printf("vendor_storage_init-004 \n");
		memset((u8 *)vendor_info.hdr, 0, NAND_VENDOR_INFO_SIZE);
		vendor_info.hdr->version = 1;
		vendor_info.hdr->tag = NVM_VENDOR_TAG;
		vendor_info.hdr->free_size = ((u32)(size_t)vendor_info.hash
			- (u32)(size_t)vendor_info.data);

		*(vendor_info.version2) = vendor_info.hdr->version;
		for (offset = 0; offset < mtdnand_info.partsize; offset += mtd->erasesize) {
			if (!nandpart_block_isbad(mtd, offset)) {
				memset(&ei, 0, sizeof(struct erase_info));
				ei.addr = mtdnand_info.blk_offset + offset;
				ei.len  = mtd->erasesize;
				ei.mtd = mtd;
				nandpart_erase(mtd, &ei);
			}
		}

		ret = mtd_vendor_nand_write(mtd, (u8 *)vendor_info.hdr);
	}
	printf("vendor_storage_init finished.\n");
	debug("[Vendor INFO]:ret=%d.\n", ret);

out:
	if (ret)
		bootdev_type = 0;

	return ret;
}

/*
 * @id: item id, first 4 id is occupied:
 *	VENDOR_SN_ID
 *	VENDOR_WIFI_MAC_ID
 *	VENDOR_LAN_MAC_ID
 *	VENDOR_BLUETOOTH_ID
 * @pbuf: read data buffer;
 * @size: read bytes;
 *
 * return: bytes equal to @size is success, other fail;
 */
int vendor_storage_read(u16 id, void *pbuf, u16 size)
{
	int ret = 0;
	u32 i;
	u16 offset;
	struct vendor_item *item;

	/* init vendor storage */
	if (!bootdev_type) {
		ret = vendor_storage_init();
		if (ret < 0)
			return ret;
	}

	item = vendor_info.item;
	for (i = 0; i < vendor_info.hdr->item_num; i++) {
		if ((item + i)->id == id) {
			debug("[Vendor INFO]:Find the matching item, id=%d\n", id);
			/* Correct the size value */
			if (size > (item + i)->size)
				size = (item + i)->size;
			offset = (item + i)->offset;
			memcpy(pbuf, (vendor_info.data + offset), size);
			return size;
		}
	}
	debug("[Vendor ERROR]:No matching item, id=%d\n", id);

	return -EINVAL;
}

/*
 * @id: item id, first 4 id is occupied:
 *	VENDOR_SN_ID
 *	VENDOR_WIFI_MAC_ID
 *	VENDOR_LAN_MAC_ID
 *	VENDOR_BLUETOOTH_ID
 * @pbuf: write data buffer;
 * @size: write bytes;
 *
 * return: bytes equal to @size is success, other fail;
 */
int vendor_storage_write(u16 id, void *pbuf, u16 size)
{
	int cnt, ret = 0;
	u32 i, next_index, align_size, alloc_size;
	u32 next_size;
	struct vendor_item *item, *next_item;
	u16 part_size, max_item_num, offset, part_num;
	u32 item_num, j;

	/* init vendor storage */
	if (!bootdev_type) {
		ret = vendor_storage_init();
		if (ret < 0)
			return ret;
	}


	// IF_TYPE_SPINAND:
		part_size = NAND_VENDOR_PART_BLKS;
		max_item_num = NAND_VENDOR_ITEM_NUM;
		part_num = NAND_VENDOR_PART_NUM;

	next_index = vendor_info.hdr->next_index;
	/* algin to 64 bytes*/
	align_size = (size + VENDOR_BTYE_ALIGN) & (~VENDOR_BTYE_ALIGN);
	if (size > align_size)
		return -EINVAL;


	/* If item already exist, update the item data */
	for (i = 0; i < vendor_info.hdr->item_num; i++) {
		item = vendor_info.item + i;
		if (item->id == id) {
			debug("[Vendor INFO]:Find the matching item, id=%d\n", id);
			alloc_size = ALIGN(item->size, 0x40);
			if (size > alloc_size){ // if new item is larger than old data, cp all follow item,then write the new item to the last item.
				if (vendor_info.hdr->free_size < align_size)
					return -1;
				offset = item->offset;
				for (j = i; j < item_num - 1; j++) {
					item = vendor_info.item + j;
					next_item = vendor_info.item + j + 1;
					item->id = next_item->id;
					item->size = next_item->size;
					item->offset = offset;
					next_size = ALIGN(next_item->size,
							  0x40);
					memcpy((vendor_info.data + offset),
						(vendor_info.data + next_item->offset),
					       next_size);
					offset += next_size;
				}
				printf("item addr 0x%x\n", item);
				item = vendor_info.item + j;
				printf("item addr 0x%x\n", item);
				item->id = id;
				item->offset = offset;
				item->size = size;
				memcpy((vendor_info.data + item->offset), pbuf, size);
				vendor_info.hdr->free_offset = offset + align_size;
				vendor_info.hdr->free_size -= (align_size -
							alloc_size);
			} else {
				memcpy((vendor_info.data + item->offset), pbuf, size);
				item->size = size;

			}
			vendor_info.hdr->version++;
			*(vendor_info.version2) = vendor_info.hdr->version;
			vendor_info.hdr->next_index++;

			ret = mtd_vendor_nand_write(mtd, (u8 *)vendor_info.hdr);
			return (ret == 0) ? size : -EIO;
		}
	}
	/*
	 * If item does not exist, and free size is enough,
	 * creat a new one
	 */
	if ((vendor_info.hdr->item_num < max_item_num) &&
	    (vendor_info.hdr->free_size >= align_size)) {
		debug("[Vendor INFO]:Create new Item, id=%d\n", id);
		item = vendor_info.item + vendor_info.hdr->item_num;
		item->id = id;
		item->offset = vendor_info.hdr->free_offset;
		item->size = size;

		vendor_info.hdr->free_offset += align_size;
		vendor_info.hdr->free_size -= align_size;
		memcpy((vendor_info.data + item->offset), pbuf, size);
		vendor_info.hdr->item_num++;
		vendor_info.hdr->version++;
		vendor_info.hdr->next_index++;
		*(vendor_info.version2) = vendor_info.hdr->version;

		ret = mtd_vendor_nand_write(mtd, (u8 *)vendor_info.hdr);
		return (ret == 0) ? size : -EIO;
	}
	debug("[Vendor ERROR]:Vendor has no space left!\n");

	return -ENOMEM;
}

/**********************************************************/
/*              vendor API uinit test                      */
/**********************************************************/
/* Reset the vendor storage space to the initial state */
void vendor_test_reset(void)
{
	u16 i, part_size, part_num;
	u32 size, offset;
	struct erase_info ei;


	//case IF_TYPE_SPINAND:
		size = NAND_VENDOR_INFO_SIZE;
		part_size = NAND_VENDOR_PART_BLKS;
		part_num = NAND_VENDOR_PART_NUM;

	/* Invalid bootdev? */
	if (!size)
		return;

	// reset  to the first slot.
	mtdnand_info.page_offset = 0;
	mtdnand_info.blk_offset = 0;		

	memset((u8 *)vendor_info.hdr, 0, size);
	vendor_info.hdr->version = 1;
	vendor_info.hdr->tag = NVM_VENDOR_TAG;
	/* data field length */
	vendor_info.hdr->free_size = (unsigned long)vendor_info.hash -
				     (unsigned long)vendor_info.data;
	*(vendor_info.version2) = vendor_info.hdr->version;

	/* write to flash. */
	for (offset = 0; offset < mtdnand_info.partsize; offset += mtd->erasesize) {
		if (!nandpart_block_isbad(mtd, offset)) {
			memset(&ei, 0, sizeof(struct erase_info));
			ei.addr = mtdnand_info.blk_offset + offset;
			ei.len  = mtd->erasesize;
			ei.mtd = mtd;
			nandpart_erase(mtd, &ei);
		}
	}

	mtd_vendor_nand_write(mtd, (u8 *)vendor_info.hdr);

}

/*
 * A total of four tests
 * 1.All items test.
 * 2.Overrides the maximum number of items test.
 * 3.Single Item memory overflow test.
 * 4.Total memory overflow test.
 */
int vendor_storage_test(void)
{
	u16 id, size, j, item_num;
	u32 total_size;
	u8 *buffer = NULL;
	int ret = 0;
	printf("[Vendor Test]: Enter vendor_storage_test\n");
	if (!bootdev_type) {
		ret = vendor_storage_init();
		if (ret) {
			printf("%s: vendor storage init failed, ret=%d\n",
			       __func__, ret);
			return ret;
		}
	}

	/*
	 * Calculate the maximum number of items and the maximum
	 * allocable memory for each item.
	 */

	//case IF_TYPE_SPINAND:
		item_num = NAND_VENDOR_ITEM_NUM;
		total_size = (unsigned long)vendor_info.hash -
			     (unsigned long)vendor_info.data;
		size = total_size / item_num;

	/* Invalid bootdev? */
	if (!total_size)
		return -ENODEV;
	/* 64 bytes are aligned and rounded down */
	if (size > 64)
		size = (size / 64) * 64;
	/* malloc memory */
	buffer = (u8 *)malloc(size);
	if (!buffer) {
		printf("[Vendor Test]:Malloc failed(size=%d)!\n", size);
		return -ENOMEM;
	}
	printf("[Vendor Test]:Test Start...\n");
	printf("[Vendor Test]:Before Test, Vendor Resetting.\n");
	if (bootdev_type != IF_TYPE_MTD)
		vendor_test_reset();

	/* FIRST TEST: test all items can be used correctly */
	printf("[Vendor Test]:<All Items Used> Test Start...\n");
	printf("[Vendor Test]:item_num=%d, size=%d.\n", item_num, size);
	/*
	 * Write data, then read the data, and compare the
	 * data consistency
	 */
	for (id = 0; id < item_num; id++) {
		memset(buffer, id, size);
		ret = vendor_storage_write(id, buffer, size);
		if (ret < 0) {
			printf("[Vendor Test]:vendor write failed(id=%d)!\n", id);
			free(buffer);
			return ret;
		}
	}
	/* Read data */
	for (id = 0; id < item_num; id++) {
		memset(buffer, 0, size);
		ret = vendor_storage_read(id, buffer, size);
		if (ret < 0) {
			printf("[Vendor Test]:vendor read failed(id=%d)!\n", id);
			free(buffer);
			return ret;
		}
		/* check data Correctness */
		for (j = 0; j < size; j++) {
			if (*(buffer + j) != id) {
				printf("[Vendor Test]:Unexpected error occurs(id=%d)\n", id);
				printf("the data content is:\n");
				print_buffer(0, buffer, 1, size, 16);

				free(buffer);
				return -1;
			}
		}
		debug("\t#id=%03d success,data=0x%02x,size=%d.\n", id, *buffer, size);
	}
	printf("[Vendor Test]:<All Items Used> Test End,States:OK\n");

	while(1);

	printf("[Vendor Test]:<All Items Used> re init,States:OK\n");
	ret = vendor_storage_init();
	/* Read data */
	for (id = 0; id < item_num; id++) {
		memset(buffer, 0, size);
		ret = vendor_storage_read(id, buffer, size);
		if (ret < 0) {
			printf("[Vendor Test]:vendor read failed(id=%d)!\n", id);
			free(buffer);
			return ret;
		}
		/* check data Correctness */
		for (j = 0; j < size; j++) {
			if (*(buffer + j) != id) {
				printf("[Vendor Test]:Unexpected error occurs(id=%d)\n", id);
				printf("the data content is:\n");
				print_buffer(0, buffer, 1, size, 16);

				free(buffer);
				return -1;
			}
		}
		debug("\t#id=%03d success,data=0x%02x,size=%d.\n", id, *buffer, size);
	}
	printf("[Vendor Test]:<All Items Used> Test End,States:OK\n");
#ifdef CONFIG_MTD_BLK
	if (bootdev_type == IF_TYPE_MTD)
		return 0;
#endif
	/*
	 * SECOND TEST: Overrides the maximum number of items to see if the
	 * return value matches the expectation
	 */
	printf("[Vendor Test]:<Overflow Items Cnt> Test Start...\n");
	/* Any id value that was not used before */
	id = item_num;
	printf("[Vendor Test]:id=%d, size=%d.\n", id, size);
	ret = vendor_storage_write(id, buffer, size);
	if (ret == -ENOMEM)
		printf("[Vendor Test]:<Overflow Items Cnt> Test End,States:OK\n");
	else
		printf("[Vendor Test]:<Overflow Items Cnt> Test End,States:Failed\n");

	/* free buffer, remalloc later */
	free(buffer);
	buffer = NULL;
	/*
	 * remalloc memory and recalculate size to test memory overflow
	 * (1) item_num > 10: Memory is divided into 10 blocks,
	 * 11th memory will overflow.
	 * (2) 10 > item_num > 1: Memory is divided into item_num-1
	 * blocks. item_num block, memory will overflow.
	 * (3) item_num = 1: size = total_size + 512 Bytes, The first
	 * block, memory will overflow.
	 * The reason to do so is to minimize the size of the memory,
	 * making malloc easier to perform successfully.
	 */
	item_num = (item_num > 10) ? 10 : (item_num - 1);
	size = item_num ? (total_size / item_num) : (total_size + 512);
	size = (size + VENDOR_BTYE_ALIGN) & (~VENDOR_BTYE_ALIGN);
	/* Find item_num value that can make the memory overflow */
	for (id = 0; id <= item_num; id++) {
		if (((id + 1) * size) > total_size) {
			item_num = id;
			break;
		}
	}
	/* malloc */
	buffer = (u8 *)malloc(size);
	if (buffer == NULL) {
		printf("[Vendor Test]:Malloc failed(size=%d)!\n", size);
		return -ENOMEM;
	}

	/* THIRD TEST: Single Item memory overflow test */
	printf("[Vendor Test]:<Single Item Memory Overflow> Test Start...\n");
	/* The value can be arbitrary */
	memset(buffer, 'a', size);
	/* Any id value that was used before */
	id = 0;
	printf("[Vendor Test]:id=%d, size=%d.\n", id, size);
	ret = vendor_storage_write(id, buffer, size);
	if (ret == size)
		printf("[Vendor Test]:<Single Item Memory Overflow> Test End, States:OK\n");
	else
		printf("[Vendor Test]:<Single Item Memory Overflow> Test End, States:Failed\n");

	/* FORTH TEST: Total memory overflow test */
	printf("[Vendor Test]:<Total memory overflow> Test Start...\n");
	printf("[Vendor Test]:item_num=%d, size=%d.\n", item_num, size);

	vendor_test_reset();
	for (id = 0; id < item_num; id++) {
		memset(buffer, id, size);
		ret = vendor_storage_write(id, buffer, size);
		if (ret < 0) {
			if ((id == item_num) && (ret == -ENOMEM)) {
				printf("[Vendor Test]:<Total memory overflow> Test End, States:OK\n");
				break;
			} else {
				printf("[Vendor Test]:<Total memory overflow> Test End, States:Failed\n");
				break;
			}
		}
		debug("\t#id=%03d success,data=0x%02x,size=%d.\n", id, *buffer, size);
	}

	/* Test end */
	printf("[Vendor Test]:After Test, Vendor Resetting...\n");
	vendor_test_reset();
	printf("[Vendor Test]:Test End.\n");
	free(buffer);

	return 0;
}
